#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

class Robot2TfListenerNode : public rclcpp::Node 
{
public:
    Robot2TfListenerNode() : Node("robot3_tf_listener") {
        
    // 创建一个 tf2_ros::Buffer 实例并设置缓存时间
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(get_clock()); // 使用节点时钟

    // 创建 TransformListener 并将其绑定到 tf_buffer_
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_, this, false);

    // 创建一个发布 Twist 消息的 Publisher
    slave_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/robot3/cmd_vel", 10);

    auto timer_callback = [this]() 
    {         
    try {
        // 从 Slave 到 Master 的变换
        geometry_msgs::msg::TransformStamped transformSM = tf_buffer_->lookupTransform("robot3", "robot1", tf2::TimePointZero);

        // 从 Master 到 Slave 的变换
        geometry_msgs::msg::TransformStamped transformMS = tf_buffer_->lookupTransform("robot1", "robot3", tf2::TimePointZero);

        // 计算两个机器人之间的距离
        double distance = sqrt(pow(transformSM.transform.translation.x, 2) +
                            pow(transformSM.transform.translation.y, 2));

        // 设置期望的距离和比例控制增益
        double desired_distance = 1;  // 设置所需的距离
        double gain = 2;  // 比例控制增益

        // 根据距离误差计算线速度和角速度
        double distance_error = distance - desired_distance;
        
        geometry_msgs::msg::Twist vel_msg;
        
        if (distance_error<=0)
        {
            vel_msg.linear.x = 0;
            vel_msg.angular.z = 0;
        }
        else
        {        
            vel_msg.linear.x = gain * distance_error;  // 线速度控制
            vel_msg.angular.z = 4.0 * atan2(transformSM.transform.translation.y,
                                                transformSM.transform.translation.x);  // 
        }

        // 发布速度消息以控制机器人
        slave_vel_pub_->publish(vel_msg);

        // 打印距离信息
        RCLCPP_INFO(this->get_logger(), "两个机器人之间的距离: %f", distance);
    }
    catch (const tf2::TransformException &ex) 
    {
        RCLCPP_ERROR(this->get_logger(), "查找变换失败: %s", ex.what());
    }
    };

    // 创建一个定时器以定期执行回调
    timer_ = this->create_wall_timer(std::chrono::milliseconds(100), timer_callback);
    
    }
  
private:
    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr slave_vel_pub_;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<Robot2TfListenerNode>());
    rclcpp::shutdown();
    return 0;
}

